#include "IInstance3D.h"



/// Set new view vector
/**	@param view - new view vector
		@param state - state to set */
void IInstance3D::SetViewVector	( cvec3r &view,		ePIE_PARAMSTATE state  ) {
	m_InstanceData.m_Transformation.SetViewVector( view );
}

/// Set new up vector
/**	@param up - new up vector
		@param state - state to set */
void IInstance3D::SetUpVector		( cvec3r &up,		ePIE_PARAMSTATE state  ) {
	m_InstanceData.m_Transformation.SetUpVector( up );
}

/// Set new right vector
/**	@param right - new right vector
		@param state - state to set */
void IInstance3D::SetRightVector	( cvec3r &right,	ePIE_PARAMSTATE state  ) {
	m_InstanceData.m_Transformation.SetRightVector( right );
}

/// Set new scale vector
/**	@param scale - new scale vector
		@param state - state to set */
void IInstance3D::SetScale			( cvec3r &scale,	ePIE_PARAMSTATE state  ) {
	m_InstanceData.m_Scale = scale;
}

/// Set new shear vector
/**	@param shear - new shear vector
		@param state - state to set */
void IInstance3D::SetShear			( cvec3r &shear,	ePIE_PARAMSTATE state  ) {
	m_InstanceData.m_Shear = shear;
}

/// Set orientation quaternion
/**	@param orient - new rotation quaternion
		@param state - state to set */
void IInstance3D::SetOrientation	( const quatr &orient,	ePIE_PARAMSTATE state  ) {
	m_InstanceData.m_Orientation = orient;
}

/// Set new position
/**	@param pos - new position
		@param state - state to set */
void IInstance3D::SetPosition		( cvec3r &pos,		ePIE_PARAMSTATE state  ) {
	m_InstanceData.m_Transformation.SetTranslate( pos );
}

/// Set new velocity
/**	@param vel - new velocity
		@param state - state to set */
void IInstance3D::SetVelocity		( const vec4r &vel,		ePIE_PARAMSTATE state  ) {
	m_InstanceData.m_Velocity = vel;
}

/// Set new spin velocity quaternion
/**	@param svel - new velocity
		@param state - state to set */
void IInstance3D::SetSpinVelocity( const quatr &svel,		ePIE_PARAMSTATE state  ) {
	m_InstanceData.m_SpinVelocity = svel;
}

/// Set new acceleration
/**	@param accel - new acceleration
		@param state - state to set */
void IInstance3D::SetAccel			( creal  &accel,	ePIE_PARAMSTATE state  ) {
	m_InstanceData.m_Acceleration = accel;
}

/// Set new spin acceleration
/**	@param saccel - new spin acceleration
		@param state - state to set */
void IInstance3D::SetSpinAccel	( creal  &saccel,	ePIE_PARAMSTATE state  ) {
	m_InstanceData.m_SpinAcceleration = saccel;
}

/// Set new transformation
/**	@param trans - new transformation
		@param state - state to set */
void IInstance3D::SetTransform	( const mat4r &trans,	ePIE_PARAMSTATE state ) {
	m_InstanceData.m_Transformation = trans;
}

/// Set new clock
/**	@param clock - new clock
		@param state - state to set */
void IInstance3D::SetClock			( creal  &clock,	ePIE_PARAMSTATE state ) {
	m_InstanceData.m_Clock = clock;
}

/// Immediate movement towards an instance of the same world
/**	@param obj - instance id inside the same world where this instance is located
		@param spd - speed of movement
		@param dtime - passed time */
void IInstance3D::StepToward		( uint obj, creal &spd, creal &dtime ) {
	//TODO
}

/// Immediate movement towards position in 3D space
/**	@param pos - position to move toward
		@param spd - speed of movement
		@param dtime - passed time */
void IInstance3D::StepTo			( cvec3r &pos, creal &spd, creal &dtime ) {
	vec3r dir = (pos - m_InstanceData.m_Transformation.GetTranslate( )).N();
	m_InstanceData.m_Transformation.ApplyTranslation( dir*spd*dtime );
}

/// Immediate movement towards normalized direction in 3d space
/**	@param dir - direction to move to
		@param spd - speed of movement
		@param dtime - passed time */
void IInstance3D::StepToward		( cvec3r &dir, creal &spd, creal &dtime ) {
	m_InstanceData.m_Transformation.ApplyTranslation( dir*spd*dtime );
}

/// Immediate movement along the current view vector. Use negative speed for reverse.
/**	@param spd - speed of movement
		@param dtime - passed time */
void IInstance3D::StepForward		( creal &spd, creal &dtime ) {
	m_InstanceData.m_Transformation.ApplyTranslation( m_InstanceData.m_Transformation.GetViewVector( )*spd*dtime );
}

/// Immediate movement along the current right vector. Use negative speed for left.
/**	@param spd - speed of movement
		@param dtime - passed time */
void IInstance3D::StepRight		( creal &spd, creal &dtime ) {
	m_InstanceData.m_Transformation.ApplyTranslation( m_InstanceData.m_Transformation.GetRightVector( )*spd*dtime );
}

/// Immediate movement along the current up vector. Use negative speed for down.
/**	@param spd - speed of movement
		@param dtime - passed time */
void IInstance3D::StepUp			( creal &spd, creal &dtime ) {
	m_InstanceData.m_Transformation.ApplyTranslation( m_InstanceData.m_Transformation.GetUpVector( )*spd*dtime );
}


/// Batch style rotation - yaw / pitch / roll
/**	@param yaw 		- rotation around Y axis 	(lookleft/lookright)
		@param pitch 	- rotation around X axis 	(lookup/lookdown)
		@param roll 	- rotation around Z axis 	(rollleft/rollright) */
void IInstance3D::YawPitchRoll	( creal &yaw, creal &pitch, creal &roll ) {
	if( !yaw && !pitch && !roll ) return;

	mat3r newtrans;
	quatr newrotation;
	newrotation.FromAxisAngle( vec3r( d2r(pitch), d2r(yaw), d2r(roll) ) );

	m_InstanceData.m_Orientation = (m_InstanceData.m_Orientation * newrotation).N();
	newtrans.Rotational( m_InstanceData.m_Orientation );
	m_InstanceData.m_Transformation = newtrans; //TODO Check if correct. Must change only rotation and must be normalized! If not correct use those below

	/*m_InstanceData.m_Transformation.SetViewVector( newtrans.GetViewVector().N() );
	m_InstanceData.m_Transformation.SetRightVector( newtrans.GetRightVector.N() );
	m_InstanceData.m_Transformation.SetUpVector( m_InstanceData.m_Transformation.GetViewVector() % m_InstanceData.m_Transformation.GetRightVector(); */
}

/// Batch style rotation - yaw / pitch
/**	@param yaw 		- rotation around Y axis 	(lookleft/lookright)
		@param pitch 	- rotation around X axis 	(lookup/lookdown)*/
void IInstance3D::YawPitch			( creal &yaw, creal &pitch ) {
	if( !yaw && !pitch ) return;

	mat3r newtrans;
	quatr newrotation;
	newrotation.FromAxisAngle( vec3r( d2r(pitch), d2r(yaw), 0 ) );

	m_InstanceData.m_Orientation = (m_InstanceData.m_Orientation * newrotation).N();
	newtrans.Rotational( m_InstanceData.m_Orientation );
	m_InstanceData.m_Transformation = newtrans; //TODO Check if correct. Must change only rotation and must be normalized! If not correct use those below

	/*m_InstanceData.m_Transformation.SetViewVector( newtrans.GetViewVector().N() );
	m_InstanceData.m_Transformation.SetRightVector( newtrans.GetRightVector().N() );
	m_InstanceData.m_Transformation.SetUpVector( m_InstanceData.m_Transformation.GetViewVector() % m_InstanceData.m_Transformation.GetRightVector() );*/
}

void IInstance3D::LookAt			( uint obj, bool automatic ) {
	//TODO
}

void IInstance3D::LookAt			( cvec3r &pos ) {
	//TODO
}

void IInstance3D::Yaw				( creal &deg ) {
	if( !deg ) return;

	mat3r newtrans;
	quatr newrotation;
	newrotation.FromAxisAngle( vec3r( 0, d2r(deg), 0 ) );

	m_InstanceData.m_Orientation = (m_InstanceData.m_Orientation * newrotation).N();
	newtrans.Rotational( m_InstanceData.m_Orientation );
	m_InstanceData.m_Transformation = newtrans; //TODO Check if correct. Must change only rotation and must be normalized! If not correct use those below

	/*m_InstanceData.m_Transformation.SetViewVector( newtrans.GetViewVector().N() );
	m_InstanceData.m_Transformation.SetRightVector( newtrans.GetRightVector.N() );
	m_InstanceData.m_Transformation.SetUpVector( m_InstanceData.m_Transformation.GetViewVector() % m_InstanceData.m_Transformation.GetRightVector(); */
}

void IInstance3D::Pitch				( creal &deg ) {
	if( !deg ) return;

	mat3r newtrans;
	quatr newrotation;
	newrotation.FromAxisAngle( vec3r( d2r(deg), 0, 0 ) );

	m_InstanceData.m_Orientation = (m_InstanceData.m_Orientation * newrotation).N();
	newtrans.Rotational( m_InstanceData.m_Orientation );
	m_InstanceData.m_Transformation = newtrans; //TODO Check if correct. Must change only rotation and must be normalized! If not correct use those below

	/*m_InstanceData.m_Transformation.SetViewVector( newtrans.GetViewVector().N() );
	m_InstanceData.m_Transformation.SetRightVector( newtrans.GetRightVector.N() );
	m_InstanceData.m_Transformation.SetUpVector( m_InstanceData.m_Transformation.GetViewVector() % m_InstanceData.m_Transformation.GetRightVector(); */
}

void IInstance3D::Roll				( creal &deg ) {
	if( !deg ) return;

	mat3r newtrans;
	quatr newrotation;
	newrotation.FromAxisAngle( vec3r( 0, 0, d2r(deg) ) );

	m_InstanceData.m_Orientation = (m_InstanceData.m_Orientation * newrotation).N();
	newtrans.Rotational( m_InstanceData.m_Orientation );
	m_InstanceData.m_Transformation = newtrans; //TODO Check if correct. Must change only rotation and must be normalized! If not correct use those below

	/*m_InstanceData.m_Transformation.SetViewVector( newtrans.GetViewVector().N() );
	m_InstanceData.m_Transformation.SetRightVector( newtrans.GetRightVector.N() );
	m_InstanceData.m_Transformation.SetUpVector( m_InstanceData.m_Transformation.GetViewVector() % m_InstanceData.m_Transformation.GetRightVector(); */
}

void IInstance3D::Look				( ) {
	//TODO
}

